#ifndef FSM_STATE_LOCOMOTION_H
#define FSM_STATE_LOCOMOTION_H

#include "FSM_State.h"
#include "common/Controllers/FootSwingTrajectory.h"
#include "srpq_controller/AdvanceControllers/convexMPC/ConvexMPCLocomotion.h"
#include "srpq_controller/AdvanceControllers/BalanceController/BalanceController.hpp"
#include "srpq_controller/AdvanceControllers/SportsController/SportsController.h"

template<typename T> class WBC_Ctrl;
template<typename T> class LocomotionCtrlData;

template <typename T>
class FSM_State_Locomotion : public FSM_State<T> {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  FSM_State_Locomotion(ControlFSMData<T>* _controlFSMData);

  // Behavior to be carried out when entering a state
  void onEnter();

  // Run the normal behavior for the state
  void run();

  // Checks for any transition triggers
  FSM_StateName checkTransition();

  // Manages state specific transitions
  TransitionData<T> transition();

  // Behavior to be carried out when exiting a state
  void onExit();

  void Stand();
  void Trot(float upAmp,float downAmp);

 private:
  // Keep track of the control iterations
  int iter = 0;

  // main controller
  ConvexMPCLocomotion* cMPCOld;
  WBC_Ctrl<T> * _wbc_ctrl;
  LocomotionCtrlData<T> * _wbc_data;
  BalanceController<T>* _bc_ctrl;
  BalanceCtrlData<T>* _bc_data;
  SportsController<T>* _sp_ctrl;

  // Parses contact specific controls to the leg controller
  void LocomotionControlStep(Leg_Mode mode);

  bool locomotionSafe();

  // Impedance control for the stance legs during locomotion
  void StanceLegImpedanceControl(int leg);

};

#endif  // FSM_STATE_LOCOMOTION_H
